package it.gius.pe.algorithm.gjk;

import java.util.Arrays;

import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.witness.Witness;

import org.jbox2d.common.Vec2;

public class Simplex {

	public static final int LARGER_2D_SIMPLEX = 3;
	public static final int LARGER_3D_SIMPLEX = 4;

	public static final int UNBOUNDED_SIMPLEX = -1;

	public static final int IS_0_SIMPLEX = 1;
	public static final int IS_1_SIMPLEX = 2;
	public static final int IS_2_SIMPLEX = LARGER_2D_SIMPLEX;

	public static final int VEC_STANDARD_UNBOUNDED_DIMENSION = 15;

	public Vec2[] vs;
	public Witness witnesses[];

	public int winding = -1;

	public int type = 0;

	public int currentDim = 0;

	public boolean correctType(int type)
	{
		return type == LARGER_2D_SIMPLEX || type == LARGER_3D_SIMPLEX || type == UNBOUNDED_SIMPLEX;
	}


	public Simplex(int type) {

		if(!correctType(type))
			throw new IllegalArgumentException();

		this.type = type;

		if(type == UNBOUNDED_SIMPLEX)
		{
			vs = new Vec2[VEC_STANDARD_UNBOUNDED_DIMENSION];
			witnesses = new Witness[VEC_STANDARD_UNBOUNDED_DIMENSION];
		}
		else
		{
			vs = new Vec2[type];
			witnesses = new Witness[type];
		}

		for(int i=0; i< vs.length;i++)
		{
			vs[i] = new Vec2();
			witnesses[i] = null;
		}
	}

	public void set(Simplex other)
	{
		if(this.vs.length < other.currentDim)
		{
			this.vs = new Vec2[other.currentDim];
			this.witnesses = new Witness[other.currentDim];
			
			for(int i=0; i<this.vs.length; i++)
			{
				vs[i] = new Vec2();
				witnesses[i] = null;
			}
		}
		
		for(int i=0; i<other.currentDim; i++)
		{
			vs[i].set(other.vs[i]);
			
			if(other.witnesses[i] != null)
				witnesses[i] = other.witnesses[i].clone();
		}
		
		this.currentDim = other.currentDim;
		
	}

	public void computeWinding()
	{
		if(currentDim <= IS_1_SIMPLEX)
			return;

		float solution = 0;

		for(int i=0;i < currentDim && solution == 0;i++)
		{
			int i1 = i;
			int i2 = (i+1) % currentDim;
			int i3 = (i+2) % currentDim;

			Vec2 vs1 = vs[i1];
			Vec2 vs2 = vs[i2];
			Vec2 vs3 = vs[i3];

			Vec2 vs21 = vs1.sub(vs2);

			Vec2 vs23 = vs3.sub(vs2);

			solution = Vec2.cross(vs21, vs23);

			//System.out.println("solution: " + solution);	
		}

		if(solution >= 0)
			winding = SystemCostants.CLOCKWISE_WINDING;
		else
			winding = SystemCostants.COUNTER_CLOCKWISE_WINDING;

	}


	public void insert(Vec2 insertPoint,Witness pointWitness, int index)
	{
		if(index >= currentDim)
			throw new IllegalArgumentException();

		if(vs.length == currentDim) //not more space left
			updateArrays();


		for(int i=currentDim; i> index;i--)
		{
			vs[i] = vs[i-1];
			witnesses[i] = witnesses[i-1];
		}
		
		currentDim++;

		vs[index] = new Vec2();
		vs[index].set(insertPoint);
		
		if(pointWitness != null)
			witnesses[index] = pointWitness.clone();
		else
			witnesses[index] = null;
		
	}



	private void updateArrays()
	{

		Vec2 newVecArray[] = new Vec2[vs.length*2];
		Witness newWitArray[] = new Witness[vs.length*2];

		for(int i=0; i < vs.length; i++)
		{
			newVecArray[i] = vs[i];
			newWitArray[i] = witnesses[i];
		}

		vs = newVecArray;
		witnesses = newWitArray;
	}




	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + currentDim;
		result = prime * result + Arrays.hashCode(vs);
		result = prime * result + Arrays.hashCode(witnesses);
		return result;
	}


	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		Simplex other = (Simplex) obj;
		if (currentDim != other.currentDim)
			return false;
		//if (!Arrays.equals(vs, other.vs))
		//return false;
		for(int i=0; i<currentDim; i++)
		{
			if(!vs[i].equals(other.vs[i]))
				return false;
			
			if(witnesses[i] == null)
			{
				if(other.witnesses[i]!= null)
					return false;
			}
			else if(!witnesses[i].equals(other.witnesses[i]))
				return false;
		}
		//if (!Arrays.equals(witnesses, other.witnesses))
		//return false;
		return true;
	}




	public static class SimplexEdge{

		public SimplexEdge() {
			normal = new Vec2();
		}

		public int indexB;

		public float distance;
		public Vec2 normal;
		
		//debug data
		public int iterations = -1;

		@Override
		public int hashCode() {
			final int prime = 31;
			int result = 1;
			result = prime * result + Float.floatToIntBits(distance);
			result = prime * result + indexB;
			result = prime * result
					+ ((normal == null) ? 0 : normal.hashCode());
			return result;
		}

		@Override
		public boolean equals(Object obj) {
			if (this == obj)
				return true;
			if (obj == null)
				return false;
			if (getClass() != obj.getClass())
				return false;
			SimplexEdge other = (SimplexEdge) obj;
			if (Float.floatToIntBits(distance) != Float
					.floatToIntBits(other.distance))
				return false;
			if (indexB != other.indexB)
				return false;
			if (normal == null) {
				if (other.normal != null)
					return false;
			} else if (!normal.equals(other.normal))
				return false;
			return true;
		}
		
		
		
		
	}

}
